#include"ros/ros.h"
#include "cloud_pose_msg/CloudPose.h"
#include "pcl_conversions/pcl_conversions.h"
#include "pcl/common/transforms.h"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
ros::Publisher cloud_pub;
void cloud_pose_cb(const cloud_pose_msg::CloudPoseConstPtr & msg){
    pcl::PointCloud<pcl::PointXYZI> cloud;
    pcl::fromROSMsg(msg->cloud,cloud);
    Eigen::Quaternion<float>qf;
    qf.x() = msg->pose.orientation.x;
    qf.y() = msg->pose.orientation.y;
    qf.z() = msg->pose.orientation.z;
    qf.w() = msg->pose.orientation.w;
    Eigen::Matrix4f trans;
    trans.setIdentity();
    trans.block<3,3>(0,0) = qf.toRotationMatrix();
    trans(0,3) = msg->pose.position.x;
    trans(1,3) = msg->pose.position.y;
    trans(2,3) = msg->pose.position.z;
    pcl::transformPointCloud(cloud,cloud,trans);
    sensor_msgs::PointCloud2 ros_cloud;
    pcl::toROSMsg(cloud,ros_cloud);
    ros_cloud.header.frame_id = "map";
    cloud_pub.publish(ros_cloud);
}
int main(int argc, char *argv[])
{
    ros::init(argc,argv,"test_align");
    ros::NodeHandle nh;
    cloud_pub = nh.advertise<sensor_msgs::PointCloud2>("cloud",100);
    ros::Subscriber cloud_pose_sub = nh.subscribe("cloud_enu",100,&cloud_pose_cb);
    ros::spin();
    return 0;
}
